#ifndef _BUOLA_CV_CVISUALSERVOING_H_
#define _BUOLA_CV_CVISUALSERVOING_H_

#include <buola/image/format.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/scene.h>
#include <buola/cv/corientedchamfer.h>
#include <buola/scene/transform/crevolutionjoint.h>

namespace buola { namespace cvi {

class CVisualServoing
{
    struct SFeature
    {
        CPoint_i mRP;
        mat::CVec4d mPos;
        CPoint_d mClosest;
        double mDist;
        double mDD[6];
        int mOri;
    };

public:
    CVisualServoing();
    virtual ~CVisualServoing();

    void Init();

    void SetCamera(const scene::PCamera &pCamera);
    const scene::PCamera &Camera()    {    return mCamera; }
    void SetScene(const scene::PScene &pScene,const scene::PRTTransform &pControlled);
    const scene::PScene &Scene()      {   return mScene;  }
    
    void AddControlledJoint(const scene::PRevolutionJoint &pJoint);
    
    void SetTarget(const img::CView_rgb8 &pTargetImage,const img::CView_gray32f &pTargetDepth);
    void SetInitialPose(const mat::CVec3d &pPos,const mat::CQuat_d &pRot);

    const mat::CVec3d &Position()         {   return mPosition;   }
    const mat::CQuat_d &Rotation()      {   return mRotation;   }
    
    void Iterate(bool pReally,bool p2D,bool p3D);

    const img::CImage_rgb8 &LastRender()     {   return mLastRender; }
    const img::CImage_gray32 &LastDepth()     {   return mLastDepth; }
    const img::CImage_gray8 &EdgesImage(int i)     {   return mChamfer->EdgesImage(i); }

    void GetDirectionsImage(const img::CMView_bgra8 &pDst,const mat::CVec3d &pPos=mat::CVec3d(0,0,0),int pSkip=20);
    void DrawRenderEdges(const img::CMView_rgb8 &pDst);
    
    img::CImage_gray8 GetMappableMask();

    const img::CView_gray8 &TransformIndex()    {   return mTransformIndex; }
    const std::vector<scene::PTransform> &TransformList()    {   return mTransforms;  }

    const mat::CMat4d &LastVPInverse()      {   return mLastVPInverse;  }
    const mat::CMat4d &LastProjection()      {   return mLastProjection;  }
private:
    void UpdateTransforms();
    void GetContoursImage(const img::CMView_rgb8 &pDst);

    CPoint_d GetPointInImage(const mat::CVec3d &pPos);

private:
    img::CImage_gray8 mTargetImage;
    img::CImage_gray32f mTargetDepth;
    img::CImage_rgb8 mLastRender;
    img::CImage_gray32 mLastDepth;
    img::CImage_gray8 mTargetDist;
    img::CImage_rgb8 mCombinedImage[8];
    img::CImage_gray8 mTransformIndex; //image of transform index for each pixel
    std::vector<scene::PTransform> mTransforms; //vector of transforms

    scene::PScene mScene;
    scene::PCamera mCamera;
    scene::PRTTransform mControlled;
    std::vector<scene::PRevolutionJoint> mControlledJoints;

    std::unique_ptr<scene::CImageRenderer> mRenderer;

    mat::CVec3d mPosition;
    mat::CQuat_d mRotation;

    std::unique_ptr<COrientedChamfer> mChamfer;
    std::vector<SFeature> mFeatures;
    
    std::vector<double> mDeltas;
    std::vector<double> mAlphas;
    std::vector<double> mBetas;

    mat::CMat4d mLastVPInverse;    
    mat::CMat4d mLastProjection;    
};

/*namespace cvi*/ } /*namespace buola*/ }

#endif
